#include <memory>

#include "rclcpp/rclcpp.hpp"
#include "cv_msg/msg/rgb_frame.hpp"
#include <opencv2/opencv.hpp>
#include <QtCore/qsharedmemory.h>

using std::placeholders::_1;

class MinimalSubscriber : public rclcpp::Node
{
public:
	MinimalSubscriber()
		: Node("node_cvsub")
	{
		subscription_ = this->create_subscription<cv_msg::msg::RgbFrame>(
			"RgbFrame", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1));


		m_sm = new QSharedMemory("sm_img");
		m_sm->create(921600*3);
	}

private:
	void topic_callback(const cv_msg::msg::RgbFrame::SharedPtr msg) const
	{
		RCLCPP_INFO(this->get_logger(), "I heard: %s", msg->serial_num.c_str());


		m_sm->lock();


		//RCLCPP_INFO(this->get_logger(), "I heard: %d", ((unsigned char*)(m_sm->data()))[1]);

		cv::Mat img = cv::Mat(480*3, 640, CV_8UC3, m_sm->data());
		cv::imshow("xxx", img);
		cv::waitKey(1);
		m_sm->unlock();
	}

	rclcpp::Subscription<cv_msg::msg::RgbFrame>::SharedPtr subscription_;
	QSharedMemory* m_sm;

};

int main(int argc, char* argv[])
{
	rclcpp::init(argc, argv);
	rclcpp::spin(std::make_shared<MinimalSubscriber>());
	rclcpp::shutdown();
	return 0;
}